% Florian Meyer, 22/04/15.

function [ measurements ] = sensor(agentPos, anchorPos, parameter)
[agentNum, ~,] = size(agentPos);
[anchorNum, ~,] = size(anchorPos);

measurementVariance = parameter.measurementVariance;

measurements = cell(2,1);
measurementsAgentsAnchors = zeros(agentNum,anchorNum);

for anchor = 1:anchorNum
    tmp = agentPos-[(anchorPos(anchor,1))*ones(agentNum,1) (anchorPos(anchor,2))*ones(agentNum,1)];
    distTmp = sqrt(tmp(:,1).^2 + tmp(:,2).^2);
    
    if(parameter.pathloss)
        idx1 = distTmp > parameter.distZero;
        idx2 = distTmp <= parameter.distZero;
        measurementVarianceTmp = zeros(agentNum,1);
        measurementVarianceTmp(idx1) = ((distTmp(idx1)/parameter.distZero - 1).^parameter.kappa + 1)*measurementVariance;
        measurementVarianceTmp(idx2) = measurementVariance;
        measurementsAgentsAnchors(:,anchor) = distTmp + sqrt(measurementVarianceTmp).*randn(agentNum,1);
    else
        measurementsAgentsAnchors(:,anchor) = distTmp + sqrt(measurementVariance).*randn(agentNum,1);
    end
    
    measurementsAgentsAnchors(:,anchor) = distTmp + sqrt(measurementVariance).*randn(agentNum,1);
end

measurementsAgentsAgents = zeros(agentNum,agentNum);

for agent = 1:agentNum
    tmp = agentPos-[(agentPos(agent,1))*ones(agentNum,1) (agentPos(agent,2))*ones(agentNum,1)];
    distTmp = sqrt(tmp(:,1).^2 + tmp(:,2).^2);
    
    
    if(parameter.pathloss)
        idx1 = distTmp > parameter.distZero;
        idx2 = distTmp <= parameter.distZero;
        measurementVarianceTmp = zeros(agentNum,1);
        measurementVarianceTmp(idx1) = ((distTmp(idx1)/parameter.distZero - 1).^parameter.kappa + 1)*measurementVariance;
        measurementVarianceTmp(idx2) = measurementVariance;
        measurementsAgentsAgents(:,agent) = distTmp + sqrt(measurementVarianceTmp).*randn(agentNum,1);
    else
        measurementsAgentsAgents(:,agent) = distTmp + sqrt(measurementVariance).*randn(agentNum,1);
    end
    
end

measurements{1} = measurementsAgentsAnchors;
measurements{2} = measurementsAgentsAgents;
end

